#include"vld_imu_slam/feature_extract.h"
#include"vld_imu_slam/show_pub.h"
#include"vld_imu_slam/frontend.h"

vld_imu_slam::FeatureExtract::Ptr p_feature_extract_;
vld_imu_slam::Frontend::Ptr p_frontend_;
vld_imu_slam::ShowPub::Ptr p_show_pub_;

deque<sensor_msgs::PointCloud2ConstPtr> pc_raw_deq_;
mutex pc_raw_deq_mutex_;

void pc_custom_CB(const sensor_msgs::PointCloud2ConstPtr &msg){
    lock_guard<mutex> lck(pc_raw_deq_mutex_);
    pc_raw_deq_.push_back(msg);
}

void frontendThread(){
    while(true){
        sensor_msgs::PointCloud2ConstPtr msg;
        {
            lock_guard<mutex> lck(pc_raw_deq_mutex_);
            if(!pc_raw_deq_.empty()){
                msg = pc_raw_deq_.front();
                pc_raw_deq_.pop_front();
            }
        }
        if(msg!=nullptr){
            p_frontend_->p_imu_handle_->setFrameTimestampInteg(msg->header.stamp.toSec());
            p_feature_extract_->pcHandle(msg);
            vld_imu_slam::Frame::Ptr p_frame(new vld_imu_slam::Frame(msg->header.stamp.toSec(),p_feature_extract_->pc_full_,
                                                                        p_feature_extract_->pc_surface_,p_feature_extract_->pc_conner_));
            SE3 imu_pose;
            if(p_feature_extract_->getPoseFromImudata(msg->header.stamp.toSec(),imu_pose)){
                p_frame->setImuPose(imu_pose);
            }
            p_feature_extract_->setGetImuPoseFlag(false);
            p_frontend_->addFrame(p_frame);

            if(p_frontend_->frontendHandle()){
                // p_show_pub_->pubMap(p_frontend_->p_map_->getMap());
                // p_show_pub_->pubMap(p_frontend_->p_last_frame_->pc_surface_);
                // p_show_pub_->pubMap(p_feature_extract_->pc_surface_);
                // p_show_pub_->pubFull(p_feature_extract_->pc_full_);
                double var[7];
                vld_imu_slam::SE3toRt(p_frontend_->p_current_frame_->getPose(),var);
                p_show_pub_->pubOdom(var);
            }
        }
        ros::Duration(0.000001).sleep();
    }
}

void getIMUCB(const sensor_msgs::Imu::ConstPtr& imu_raw){
    p_frontend_->p_imu_handle_->pushbackIMUData(vld_imu_slam::imuConverter2Lidar(*imu_raw,p_frontend_->p_config_));
}

void imuIntegrationThread(){
    deque<nav_msgs::Odometry> deq_odom;
    while(true){
        cout<<"imuIntegrationThread"<<endl;
        deq_odom = p_frontend_->p_imu_handle_->imuIntegration();
        p_feature_extract_->getImuPoseEstimate(deq_odom);
        while(!deq_odom.empty()){
            p_show_pub_->pubImuIntegOdom(deq_odom.front());
            deq_odom.pop_front();
        }
    }
}

void imuPreintegrationThread(){
    while(true){
        cout<<"imuPreintegrationThread"<<endl;
        p_frontend_->p_imu_handle_->imuPreintegration();
        ros::Duration(0.00001).sleep();
    }
}

void loopDetectThread(){
    while(true){
        cout<<"loopthread"<<endl;
        p_frontend_->p_map_->loopThread();
        ros::Duration(1).sleep();
    }
}

void showThread(){
    while(true){
        p_show_pub_->pubMap(p_frontend_->p_map_->getMap());
        ros::Duration(5).sleep();
    }
}


int main(int argc, char** argv)
{
    if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug))
    {
        ros::console::notifyLoggerLevelsChanged();
    }

    ros::init(argc, argv, "vld_imu_slam");
    ros::NodeHandle nh;

    vld_imu_slam::Config::Ptr p_config(new vld_imu_slam::Config(nh));
    p_feature_extract_.reset(new vld_imu_slam::FeatureExtract(p_config));

    p_frontend_.reset(new vld_imu_slam::Frontend(p_config));

    p_show_pub_.reset(new vld_imu_slam::ShowPub(nh));

    ros::Subscriber pc_sub_ = nh.subscribe<sensor_msgs::PointCloud2>(p_config->pointcloud_topic_, 5, pc_custom_CB, ros::TransportHints().tcpNoDelay());
    ros::Subscriber imu_sub_ = nh.subscribe<sensor_msgs::Imu>(p_config->imu_topic_, 5, getIMUCB, ros::TransportHints().tcpNoDelay());
    
    thread frontend_thread(frontendThread);
    frontend_thread.detach();

    thread imu_integ_thread(imuIntegrationThread);
    imu_integ_thread.detach();

    thread imu_preinteg_thread(imuPreintegrationThread);
    imu_preinteg_thread.detach();

    thread loop_detect_thread(loopDetectThread);
    loop_detect_thread.detach();

    thread show_thread(showThread);
    show_thread.detach();

    ros::MultiThreadedSpinner spinner(2);
    spinner.spin();
    // ros::spin();

    return 0;
}
